#include <tld_utils.h>


void drawBox(Mat& image, CvRect box, Scalar color, int thick){
  rectangle( image, cvPoint(box.x, box.y), cvPoint(box.x+box.width,box.y+box.height),color, thick);
} 

void drawPoints(Mat& image, vector<Point2f> points,Scalar color){
  for( vector<Point2f>::const_iterator i = points.begin(), ie = points.end(); i != ie; ++i )
      {
      Point center( cvRound(i->x ), cvRound(i->y));
      circle(image,*i,2,color,1);
      }
}

Mat createMask(const Mat& image, CvRect box){
  Mat mask = Mat::zeros(image.rows,image.cols,CV_8U);
  drawBox(mask,box,Scalar::all(255),CV_FILLED);
  return mask;
}

float median(vector<float> v)
{
    int n = floor(v.size() / 2);
    nth_element(v.begin(), v.begin()+n, v.end());
    return v[n];
}

vector<int> index_shuffle(int begin,int end){
  vector<int> indexes(end-begin);
  for (int i=begin;i<end;i++){
    indexes[i]=i;
  }
  random_shuffle(indexes.begin(),indexes.end());
  return indexes;
}

/*method : 1 CV_TM_CCORR_NORMED  2 CV_TM_CCOEFF_NORMED*/
float myTemplateMatch(const Mat *pTemplate,const Mat *src,int method )
{
   int i, j, m, n;
   float dSumT,dSumTT;
   float dSumS,dSumSS;
   float dSumST;
   float R;

   float MaxR;

   int nMaxX;
   int nMaxY;
   int nHeight = src->rows;
   int nWidth = src->cols;

   int nTplHeight = pTemplate->rows;
   int nTplWidth = pTemplate->cols;

   if (method == 1)
   {
       dSumT = 0;
       for (m = 0; m < nTplHeight; m++)
       {
           for (n = 0; n < nTplWidth; n++)
           {
               float nGray =*pTemplate->ptr<float>(m, n);
               dSumT += nGray*nGray;
           }
       }

       MaxR = 0;
       for (i = 0; i < nHeight - nTplHeight + 1; i++)
       {
           for (j = 0; j < nWidth - nTplWidth + 1; j++)
           {
               dSumST = 0;
               dSumS = 0;
               for (m = 0; m < nTplHeight; m++)
               {
                   for (n = 0; n < nTplWidth; n++)
                   {
                       float nGraySrc = *src->ptr<float>(i + m, j + n);
                       float nGrayTpl = *pTemplate->ptr<float>(m, n);
                       dSumS += nGraySrc*nGraySrc;
                       dSumST += nGraySrc*nGrayTpl;
                   }
               }
               R = dSumST / (sqrt(dSumS)*sqrt(dSumT));
               MaxR = R;
           }
       }
   }
   else if (method == 2)
   {
       dSumT = 0;
       for (m = 0; m < nTplHeight; m++)
       {
           for (n = 0; n < nTplWidth; n++)
           {
               float nGray =*pTemplate->ptr(m, n);
//                printf("nGray:%f\n", nGray);
               dSumT  += (float)nGray;
           }
       }
       dSumT = dSumT/(nTplHeight*nTplWidth);
//        printf("dSumT:%f\n", dSumT);
       MaxR = 0;
       for (i = 0; i < nHeight - nTplHeight + 1; i++)
       {
           for (j = 0; j < nWidth - nTplWidth + 1; j++)
           {
               dSumS = 0;
               dSumSS =0;
               dSumTT =0;
               dSumST = 0;
               for (m = 0; m < nTplHeight; m++)
               {
                   for (n = 0; n < nTplWidth; n++)
                   {

                       float nGraySrc = *src->ptr(i + m, j + n);
                       dSumS += nGraySrc;
                   }
               }
               dSumS = dSumS/(nTplHeight*nTplHeight);
               for (m = 0; m < nTplHeight; m++)
               {
                   for (n = 0; n < nTplWidth; n++)
                   {

                       float nGraySrc = *src->ptr(i + m, j + n);

                       float nGrayTpl = *pTemplate->ptr(m, n);

                       float TT = (nGrayTpl-dSumT) ;
                       float SS = (nGraySrc-dSumS) ;
                       dSumTT  += TT*TT;
                       dSumSS  += SS*SS;
                       dSumST += (TT*SS);
                   }
               }
//                printf("dSumT:%f  %f   %f\n", dSumTT, dSumSS, dSumST);
               R= dSumST/(sqrt(dSumTT*dSumSS));
               MaxR = R;
           }
       }

   }
   return MaxR;
}
